Control de trayectoria de un robot esferico de tres grados de libertad (página 2)

Partes: 1, 2

f(C3,q(2))))
T2=simple(DD2*ddq+cor2+h2)
%Fuerza 3
%——–
DD3=d(3,1:3)
cor3=0; h3=0;
for k=1:3
for j=1:3
cor3=simple(cor3+(diff(d(3,j),q(k))-
1/2*diff(d(k,j),q(3)))*dq(k)*dq(j));
end
end
cor3
h3=simple(-[0 0 –
g]*(m1*diff(C1,q(3))+m2*diff(C2,q(3))+m3*diff(C3,q(3))))
F3=simple(DD3*ddq+cor3+h3)

El torque ejercido por el robot en su eje de
articulación para el eslabón 1 es:
T1=1/3*cos(q2)^2*(m2*L2^2+3*m3*r^2-
3*m3*r*L3+m3*L3^2)*ddq1-
2/3*cos(q2)*(m2*L2^2+3*m3*r^2-
3*m3*r*L3+m3*L3^2)*sin(q2)*dq2*dq1+1/3*cos(q2)^2*(6*
m3*r-3*m3*L3)*dr*dq1

Para el eslabón 2 es:
T2=(1/3*m2*L2^2+m3*r^2-
m3*r*L3+1/3*m3*L3^2)*ddq2+1/3*cos(q2)*(m2*L2^2+3*m3
*r^2-3*m3*r*L3+m3*L3^2)*sin(q2)*dq1^2+(2*m3*r-
m3*L3)*dr*dq2+1/2*g*cos(q2)*(m2*L2+2*m3*r-m3*L3)

La fuerza ejercida por el eslabón 3 hacia su eje de
traslación es:
F3=m3*ddr-1/2*m3*(2*r-
L3)*(cos(q2)^2*dq1^2+dq2^2)+g*m3*sin(q2)

2) Control Monoarticular.
Consiste en controlar cada motor
independientemente, sin tomar en cuenta las
fuerzas y torques que los demás eslabones ejercen
sobre cada motor.

En Simulink están los siguientes bloques de
izquierda a derecha:

UNIVERSIDAD NACIONAL DE INGENIERÍA
•

•

•

•

•
Analisis y Control de Robots

Generador de trayectorias cartesianas, una
línea recta en el espacio.

Transformación de coordenadas cartesianas a
articulares, usando la cinemática inversa

Control PID discreto para cada motor.

Modelamiento con bloques continuos para los
tres motores.

Modelamiento del robot por dinámica directa
que produce los torques y fuerzas perturbares.
El esquema en Simulink es el siguiente:

2.1) Generador de Trayectorias Cartesianas.
El extremo del robot recorrerá una línea recta
desde el punto A al punto B, siendo el tiempo de
simulación de 12 segundos.

PA = (0.65,0,0.9)m
PB = (0.4,0.8,1.2)m
Cálculo de velocidades:
0 . 4-0 . 65
12
0 . 8-0
12
1 . 2-0 . 9
12
Por tanto las ecuaciones que describen el
movimiento en metros, aplicando la fórmula MRU
distancia=velocidad*tiempo, son:

-6 –

Y
PX = -0.020833*t +0.65
P = 0.066666*t
P Z = 0.025*t +0.9
El bloque en simulink es:
2.2) Transformación
de
Coordenadas
Cartesianas a Articulares.

La referencia que le debe llegar a cada motor debe
estar en coordenadas articulares, por tanto se debe
emplear la cinemática inversa para pasar
coordenadas en metros a rad para el motor 1 y 2 y
milímetros para el motor 3.
Las ecuaciones que describen la cinemática inversa
son obtenidas por métodos geométricos, estas son:
)
)
2 Y
P
PX
Y
P Z -L1
PX + P2
q2 = arctag(

P Z -L1
r =
sin(q2)
q1 = arctag(
El bloque MatLab Function llama a la función
Cinv.m que calcula las coordenadas articulares en
cada iteración.

UNIVERSIDAD NACIONAL DE INGENIERÍA
Analisis y Control de Robots

2..3) Control PID discreto para cada motor

Los tres controladores fueron diseñados por el
método del Lugar Geométrico de las Raíces. El
PID discreto tiene la siguiente forma:
kC * (z -a) * (z -b)
z*(z -1)
C(Z) =
Los parámetros de diseño escogidos para los tres
motores son un tiempo de establecimiento de
0.2seg, un sobreimpulso máximo de 2% y un
periodo de muestreo de 0.001seg. ante entradas
escalón. Los controladores hallados son.
=

=
3340 * (z -0.9943) * (z -0. 974767 )
z*(z -1)
12000 * (z -0. 976093) * (z-0.9186)
z*(z-1)
3340 * (z -0.9943) * (z -0. 974767 )
z*(z -1)
C2(Z)

C3(Z)
C1(Z) =
Los motores 1 y 2 reciben el error de posición en
radianes y entregan voltaje, mientras que el motor
3 recibe el error en milímetros y entregan voltaje.

Cada control tiene su limitador de voltaje a +-
100vdc y su ZOH con un tiempo igual al tiempo de
muestreo de 0.001seg. Abriendo el bloque “PID
discreto” de color naranja, tenemos:
2.4) Modelamiento de los motores DC
El motor de corriente continua empleado en cada
articulación es:

Marca: Baldor
Voltaje=100v
TorqueNominal=4N*m
Potencia: 0.8Hp

-6 –

Los motores de corriente continua fueron
modelados con bloques de suma, multiplicación e
integrales de acuerdo a sus parámetros principales.
Reciben como entrada el voltaje en voltios y el
torque perturbador en N*m, y las salidas son la
posición en radianes, la velocidad en radianes por
segundo en el eje del engranaje (no del piñón) y la
corriente. Los parámetros del motor son:

Ra = 2?
La =1.8mH
J =8*10-3kg*m2
B = 4.94*10-3N *m/(rad /s)
kV = 0.3334v/(rad /s)
kt = 0.3334N *m/ A

Cada motor está conectado a una caja reductora
cuyos factores de engranaje son:

N1 =8
N2 =12
N3 = 2
El bloque que usa simulink es este, se puede
apreciar las entradas y salidas.
En el bloque interno se puede ver las ecuaciones
que describen el movimiento de un motor, la de
armadura y la de torque, en forma de bloques.

UNIVERSIDAD NACIONAL DE INGENIERÍA
Analisis y Control de Robots

2.5) Modelamiento del Robot
La consigna de este bloque es obtener los torques
perturbadores que produce el robot en cada
articulación. Estos torques que produce el robot
dependen de las posiciones, velocidades y
aceleraciones que ocurren en cada articulación y en
todo momento, por tanto se calculará la dinámica
directa.
El bloque MatLab Function llama a la función
DIN.m para el calculo de la dinámica directa.

El desarrollo de la dinámica directa se basa en la
siguiente expresión:

Ti F i = Hi(q) *q'' +Ci(q,q') +hi(q)
Donde:
• Hi(q) es el tensor de inercial del eslabón i,
por tanto el torque que produce la expresión
Hi(q) *q''solo produce torque o fuerza
cuando hay aceleraciones en los eslabones.
•

•
Ci(q,q') representa el torque o fuerza de
Coriolis o centrípetas y ocurre cuando hay
velocidades en las articulaciones.
hi(q)es el torque o fuerza ejercida por la
gravedad en cada articulación, esta actúa en
todo instante y depende de la posición de cada
eslabón del robot.
Se muestra el archivo DIN.m a continuación:

% tensores inerciales
H11=1/3*cos(q2)^2*(m2*L2^2+3*m3*r^2-
3*m3*r*L3+m3*L3^2);
H22=1/3*m2*L2^2+m3*r^2-m3*r*L3+1/3*m3*L3^2;
%H33=m3;
H33=I3;

% coriolis
cor1=-2/3*cos(q2)*(m2*L2^2+3*m3*r^2-
3*m3*r*L3+m3*L3^2)*sin(q2)*dq2*dq1+1/3*cos(q2)^2*(6*m
3*r-3*m3*L3)*dr*dq1;
cor2=1/3*cos(q2)*(m2*L2^2+3*m3*r^2-
3*m3*r*L3+m3*L3^2)*sin(q2)*dq1^2+(2*m3*r-
m3*L3)*dr*dq2;
%cor3=-1/2*m3*(2*r-L3)*(cos(q2)^2*dq1^2+dq2^2);

-6 –

UNIVERSIDAD NACIONAL DE INGENIERÍA
-6 –
cor3=0;

% efecto de la gravedad
h1=0;
h2=1/2*g*cos(q2)*(m2*L2+2*m3*r-m3*L3);
%h3=g*m3*sin(q2);
h3=0;

% rozamiento viscoso
Fv1=0;
Fv2=0;
Fv3=B3*dq3;

% torques perturbadores que entran a los motores
Tp1=1/N1*(H11*ddq1+cor1+h1+Fv1);
Tp2=1/N2*(H22*ddq2+cor2+h2+Fv2);
% sist. con tornillo, no coriolis, no gravedad, solo I3 y B3
Tp3=1/N3*(H33*ddq3+cor3+h3+Fv3);

Vemos en el código de arriba dos detalles muy
importantes.

Primero, los torques perturbadores están divididos
por el factor de la caja reductora de cada motor, es
decir el motor recibe una fracción del torque
perturbador que hay en cada eje del articulación.

Segundo, el motor del eslabón 3 recibe torque y no
fuerza, esto es debido a que es eslabón 3 está
formado por un tornillo sin fin y una nuez que se
desplaza, por tanto los efectos de Coriolis y
gravedad serán ceros, y el tensor inercial será
constante, además hay que agregar el rozamiento
viscoso que todo tornillo produce.

3) Resultados de la Simulación

El tiempo de simulación a usar será de tres
segundos, que nos permitirá ver la evolución del
error en el tiempo para los tres motores. Recordar
que el error es medido y controlado respecto al eje
de articulación de cada eslabón del robot y no
respecto al eje del motor.

La gráfica del error cometido por el motor en la
articulación 1 es como máximo 0.006 rad bastante
pequeño y el error en régimen permanente es cero,
debido a que el sistema es de tipo II puede hacer el
error cero ante entradas tipo rampas.
Analisis y Control de Robots

El error cometido por el motor en la articulación 2
en radianes es:

El error cometido por el motor en la articulación 3
en milímetros es:

4) Recomendaciones.
Se recomienda diseñar los controladores PID
discretos para que respondan en forma
sobreamortiguada, es decir con un factor de
amortiguamiento de 1 (e=1) o Mp=0 (máximo
sobreimpulso), con esto se reducirán las
oscilaciones que se ven en el motor 3 del eslabón
de traslación.

5) Conclusiones
•

•
Aplicar un control monoarticular, sin tener
presentes en el control las perturbaciones que
produce el robot, es recomendable cuando se
tengan motores con caja de engranajes, ya sea
interna o externa a él, para que disminuyan el
torque perturbador producido por el robot.
También este método es aplicable cuando las
masas no son considerables y las aceleraciones
y velocidades no son muy altas.

Usar la simulación para el motor DC es una
herramienta importante que acerca las

Analisis y Control de Robots
UNIVERSIDAD NACIONAL DE INGENIERÍA
-6 –
•

•
respuestas de simulación a las respuestas
reales, siempre y cuando el robot sea
construido con una mecánica fina y el puente
de H para los motores esté muy bien diseñado.

Se analizó el control desde el punto de vista
del motor, es decir se controla al motor, en el
eje de salida de la articulación, y no al robot.
Es mejor y más real obtener la posición desde
el motor que desde el robot por dinámica
inversa.

Para obtener menos error en el estado
transitorio en los motores es necesario aplicar
una aceleración tipo rampa. En este caso la
aceleración es infinita para alcanzar la
velocidad nominal.

6) Referencias
[1]
Anibal Ollero Baturone “ROBÓTICA
Manipuladores y Robots Móviles” 2001

[2] Antonio Barrientos “Fundamentos de
Robótica” Segunda edición.